#include "navigate_to_shelf.h"

// 构造函数不变，初始化定时器
NavigateToShelf::NavigateToShelf(const std::string& name, const BT::NodeConfig& config,
                                 rclcpp::Node* node)
    : BT::StatefulActionNode(name, config),
      node_(node),
      navigation_complete_(false),
      timer_(nullptr)
{
}

// 端口定义不变（输入端口shelf_id，用于接收XML映射的黑板值）
BT::PortsList NavigateToShelf::providedPorts()
{
    return {BT::InputPort<int>("shelf_id", "目标货架ID")};
}

BT::NodeStatus NavigateToShelf::onStart()
{
    // 关键1：用getInput()读“节点局部黑板”的shelf_id（XML已将全局的target_shelf映射到这）
    auto shelf_id_res = getInput<int>("shelf_id");
    if (!shelf_id_res)
    {
        // 错误信息会明确提示映射问题，方便定位
        RCLCPP_ERROR(node_->get_logger(), "获取货架ID失败：%s", shelf_id_res.error().c_str());
        return BT::NodeStatus::FAILURE;
    }
    int target_shelf = shelf_id_res.value();

    RCLCPP_INFO(node_->get_logger(), "开始导航到货架, 货架 %d", target_shelf);
    navigation_complete_ = false;

    // 定时器逻辑不变（成员变量避免悬空）
    timer_ = node_->create_wall_timer(std::chrono::seconds(2),
                                      [this]()
                                      {
                                          RCLCPP_INFO(node_->get_logger(), "到达目标货架");
                                          navigation_complete_ = true;
                                          timer_->cancel();
                                      });

    return BT::NodeStatus::RUNNING;
}

BT::NodeStatus NavigateToShelf::onRunning()
{
    return navigation_complete_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
}

void NavigateToShelf::onHalted()
{
    if (timer_)
    {
        timer_->cancel();
        timer_ = nullptr;
    }
    RCLCPP_WARN(node_->get_logger(), "导航任务被中断");
}